﻿#pragma once
#include "adaptive_surface_filter.h"
#include "low_outlier_filter.h"

#include <pcl/filters/impl/extract_indices.hpp>
#include <pcl/common/common.h>
#include <pcl/common/io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include "pyramid_exclusive.h"
#include "spline_interpolation.h"
#include "raster_surface.h"

#include <map>
#include <set>
namespace oil
{

template <typename PointT>
oil::AdaptiveSurfaceFilter<PointT>::AdaptiveSurfaceFilter(double max_window, double min_window)
{
	max_window_ = max_window;
	min_window_ = min_window;
	threshold_ = 0.3;
	filter_name_ = "AdaptiveSurfaceFilter";
	max_lambda_ = 0.5;
	max_bending_gain_ = 0.5;
	max_scale_gain_ = 0.3;
	bending_level_ = 5;
	step_size_ = 1.2;
	m_ = 5;
	k_ = 3;
	
	detect_low_outlier_ = true;
}

template <typename PointT>
void oil::AdaptiveSurfaceFilter<PointT>::setParameters(const ASFParameter& param)
{
	max_window_ = param.max_window_;
	min_window_ = param.min_window_;
	threshold_ = param.threshold_;
	max_lambda_ = param.max_lambda_;
	max_bending_gain_ = param.max_bending_gain_;
	max_scale_gain_ = param.max_scale_gain_;
	bending_level_ = param.bending_level_;
	step_size_ = param.step_size_;
	m_ = param.m_;
	k_ = param.k_;
	detect_low_outlier_ = param.detect_low_outlier_;
	low_outlier_mophological_window_ = param.low_outlier_mophological_window_;
	low_outlier_threshold_ = param.low_outlier_threshold_;
	low_outlier_mophological_size_ = param.low_outlier_mophological_size_;
	max_low_grids_ = param.max_low_grids_;
}


template <typename PointT>
oil::AdaptiveSurfaceFilter<PointT>::~AdaptiveSurfaceFilter(void)
{

}



template <typename PointT>
void oil::AdaptiveSurfaceFilter<PointT>::applyFilter(PointCloud &output)
{
	int duplicate_num, outlier_num, total_num, ground_num, non_ground_num;

	pcl::PointCloud<pcl::PointXYZ>::Ptr input_copy(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(*input_, *input_copy);
	IndicesPtr input_index(new vector<int>);
	input_index->resize(input_->size());
	iota(begin(*input_index), end(*input_index), 0);

	total_num = input_->size();

	//计算点云统计信息，最大最小值，点云密度
	Eigen::Vector4f min_p, max_p;
	pcl::getMinMax3D(*input_, *input_index, min_p, max_p);

	float area = (max_p.x() - min_p.x()) * (max_p.y() - min_p.y());
	float density = area / input_index->size();
	float min_window = sqrt(density);
	if (min_window < min_window_)
	{
		min_window = min_window_;
	}
	IndicesPtr non_outlier(new vector<int>);

	if (detect_low_outlier_)
	{
		LowOutlierFilter<PointXYZ> low_filter;
		low_filter.setWindowSize(low_outlier_mophological_window_);
		low_filter.setLowThreshold(low_outlier_threshold_);
		low_filter.setMaxLowGrids(max_low_grids_);
		low_filter.setMophologicalSize(low_outlier_mophological_size_);

		low_filter.setInputCloud(input_copy);
		low_filter.setIndices(input_index);
		low_filter.setNegative(false);
		low_filter.filter(*non_outlier);
	}
	else
	{
		*non_outlier = *input_index;
		outlier_num = 0;
	}
	

	int max_index = *(max_element(begin(*non_outlier), end(*non_outlier)));

	//计算点云金字塔
	vector<vector<int>> indices_pyramid;

	PyramidExclusive<PointT> pyramid(min_window, max_window_, step_size_);
	pyramid.setInputCloud(input_);
	pyramid.setIndices(non_outlier);
	pyramid.compute(indices_pyramid);
	auto windows = pyramid.getStepLevels();
	double max_window;
	max_window = windows[0];

	//建立ground 和 non_ground的临时变量
	IndicesPtr ground(new vector<int>);
	IndicesPtr unclassified(new vector<int>);

	//将底层全部设成ground
	*ground = indices_pyramid[0];


	//interpolation based filter的一个特性是，一直会有一个DEM留存着在
	shared_ptr<RasterSurface> bending_energy;
	RasterSurface bending; //bend gain
	shared_ptr<RasterSurface> dem;
	shared_ptr<RasterSurface> mask;

	//一层层的滤金字塔中的点
	for (int i = 1; i <= windows.size(); ++i)
	{
		//在最后一个迭代当中，只重新计算点云，然后滤波一次
		//因为这一次已经将所有的金字塔层级都计算完了，只是针对未加入金字塔的点进行滤波
		bool is_last_iteration = false;
		bool last_stop = false; //最后一级，滤波一次后终止
		double window_size;
		int thresh_num = 0;
		if (i != windows.size())
		{
			window_size = windows[i];
			thresh_num = 100;
		}
		else
		{
			window_size = windows[i - 1];
			thresh_num = 10;
			is_last_iteration = true;
		}

		double lambda = max_lambda_ * (max_window - window_size) / (max_window - min_window);
		double scale_gain = max_scale_gain_ * (window_size - min_window) / (max_window - min_window);

		std::vector<int> next = indices_pyramid[i];
		unclassified->insert(end(*unclassified), begin(next), end(next));
		bool first = true;

		std::vector<int> new_ground;
		int left_points = unclassified->size();
		int iterations = 0;

		//TPS interpolation -> raster
		SplineInterpolation interpolator;
		interpolator.setLambda(lambda);
		interpolator.setMin3D(min_p);
		interpolator.setMax3D(max_p);
		interpolator.setSearchSize(12);
		interpolator.setWindowSize(window_size);

		vector<int> changed_grids;
		while ((new_ground.size() > thresh_num || first) && !last_stop && iterations < 10)
		{

			// 提取ground的点
			PointCloud::Ptr ground_cloud(new PointCloud);
			pcl::ExtractIndices<PointT> extractor;
			extractor.setInputCloud(input_);
			extractor.setIndices(ground);
			extractor.setNegative(false);
			extractor.filter(*ground_cloud);

			//copy only xyz
			pcl::PointCloud<pcl::PointXYZ>::Ptr ground_xyz(
				new pcl::PointCloud<pcl::PointXYZ>());
			pcl::copyPointCloud(*ground_cloud, *ground_xyz);

			
			interpolator.setInputCloud(ground_xyz);	
			if (first)
			{
				interpolator.compute();
			}
			else
			{
				interpolator.update(changed_grids);
			}
			first = false;

			//最后一次迭代的话，进行一次就中指
			if (is_last_iteration)
			{
				last_stop = true;
			}

			bending_energy = interpolator.bending_energy_;
			dem = interpolator.height_surface_;
			mask = interpolator.bending_mask_;

			bending = *bending_energy;

			//根据bending energy 计算对应的修正值,采用分段线性内插的方式
			{
				transformBendingEnergy(&bending);
			}

			//filter
			{
				std::vector<int> changed_sets;
				new_ground.clear();
				new_ground.reserve(unclassified->size());
				int max_grid_id = dem->rows_ * dem->cols_ - 1;
				
				changed_sets.reserve(unclassified->size());
				
			#pragma omp parallel for
				for (int k = 0; k < unclassified->size(); ++k)
				{
					int idx = (*unclassified)[k];
					const PointT& pt = (*input_)[idx];

					int row, col;
					row = int((pt.y - min_p.y()) / window_size + 0.5);
					col = int((pt.x - min_p.x()) / window_size + 0.5);

					double bending_gain = (bending)((double)pt.x, (double)pt.y);
					double mask_value = (*mask)(row, col);
					double t = bending_gain * mask_value + scale_gain + threshold_;

					int accept_num = 0;

					std::vector<int> tmp_set;
					tmp_set.reserve(9);
					for (int r = -1; r <= 1; ++r)
					{
						for (int c = -1; c <= 1; ++c)
						{
							double z_value = (*dem)(row + r, col + c);
							if (pt.z - z_value < t)
								accept_num++;

							int id = col + c + (row + r) * dem->cols_;
							id = id >= 0 ? id : 0;
							id = id <= max_grid_id ? id : max_grid_id;
							tmp_set.push_back(id);
						}
					}

					if (accept_num >= 5)
					{
						#pragma omp critical
						{
							new_ground.push_back(idx);
							changed_sets.insert(end(changed_sets), begin(tmp_set), end(tmp_set));
						}
					}
				}

				//changed sets去重复
 				sort(changed_sets.begin(), changed_sets.end());
 				changed_sets.erase(unique(changed_sets.begin(), changed_sets.end()), changed_sets.end());
				changed_grids = changed_sets;
			}

			//将new_ground 加到ground 并且从unclassified移除
			ground->insert(end(*ground), begin(new_ground), end(new_ground));
			/*cout << "\tadd " << new_ground.size() << " points to ground at level " <<
				i << ", iterations " << iterations << endl;*/
			std::sort(begin(new_ground), end(new_ground));
			std::sort(begin(*unclassified), end(*unclassified));

			const auto& new_end = std::set_difference(begin(*unclassified),
				end(*unclassified), begin(new_ground), end(new_ground), begin(*unclassified));
			unclassified->erase(new_end, end(*unclassified));

			++iterations;
		}
	}

	//后处理，将outlier也分类
	{
		double window_size = min_window;
		
		IndicesPtr outlier_index = IndicesPtr(new vector<int>);
		outlier_index->resize(input_->size() - non_outlier->size());
		auto end_pos = set_difference(begin(*input_index), end(*input_index),
			begin(*non_outlier), end(*non_outlier), begin(*outlier_index));

		//滤波
		std::vector<int> new_ground;
		new_ground.reserve(outlier_index->size());
		vector<int> new_nonground;
		new_nonground.reserve(outlier_index->size());
		vector<int> new_lowoutlier;
		new_lowoutlier.reserve(outlier_index->size());

		for_each(begin(*outlier_index), end(*outlier_index), [&, this](int& idx){
			const PointT& pt = (*input_)[idx];
			int row, col;
			row = int((pt.y - min_p.y()) / window_size + 0.5);
			col = int((pt.x - min_p.x()) / window_size + 0.5);

			double bending_gain = (bending)((double)pt.x, (double)pt.y);
			double mask_value = (*mask)(row, col);

			double t = bending_gain * mask_value + threshold_;

			int accept_num = 0;
			int low_num = 0;
			for (int r = -1; r <= 1; ++r)
			{
				for (int c = -1; c <= 1; ++c)
				{
					double z_value = (*dem)(row + r, col + c);
					if (pt.z - z_value < t)
						accept_num++;
					if (pt.z - z_value < -0.5)
						low_num++;
				}
			}
			if (low_num >= 5)
			{
				new_lowoutlier.push_back(idx);
			}
			else if (accept_num >= 5)
				new_ground.push_back(idx);
			else
				new_nonground.push_back(idx);
		});
		ground->insert(end(*ground), begin(new_ground), end(new_ground));
		unclassified->insert(end(*unclassified), begin(new_nonground), end(new_nonground));
		sort(begin(new_lowoutlier), end(new_lowoutlier));
		sort(begin(*ground), end(*ground));
		sort(begin(*unclassified), end(*unclassified));

		low_outlier_index_ = new_lowoutlier;		
	}

	ground_num = ground->size();
	non_ground_num = input_index->size() - ground_num;

	ground_index_ = *ground;
	nonground_index_ = *unclassified;

	dem_ = dem;
	//extract output
	pcl::ExtractIndices<PointT> extract_ground;
	extract_ground.setInputCloud(input_);
	extract_ground.setIndices(ground);
	extract_ground.setNegative(false);
	extract_ground.filter(output);
}

template <typename PointT>
vector<int> oil::AdaptiveSurfaceFilter<PointT>::evaluate(vector<int>& ref_be, vector<int>& ref_obj, vector<int>& flt_be, vector<int>& flt_obj)
{
	sort(begin(ref_be), end(ref_be));
	sort(begin(ref_obj), end(ref_obj));
	sort(begin(flt_be), end(flt_be));
	sort(begin(flt_obj), end(flt_obj));

	vector<int> be_true(flt_be.size());
	vector<int> obj_ture(flt_obj.size());

	auto be_true_end = set_intersection(begin(flt_be), end(flt_be), begin(ref_be),
		end(ref_be), begin(be_true));
	auto obj_ture_end = set_intersection(begin(flt_obj), end(flt_obj), begin(ref_obj),
		end(ref_obj), begin(obj_ture));

	int be_be = distance(begin(be_true), be_true_end);
	int obj_be = flt_be.size() - be_be;

	int obj_obj = distance(begin(obj_ture), obj_ture_end);
	int be_obj = flt_obj.size() - obj_obj;

	vector<int> res;
	res.push_back(be_be);
	res.push_back(be_obj);
	res.push_back(obj_be);
	res.push_back(obj_obj);

	return res;
}

template <typename PointT>
void oil::AdaptiveSurfaceFilter<PointT>::transformBendingEnergy(RasterSurface* bending)
{
	vector<int> bending_level(bending_level_ + 1);
	vector<double> bending_interval(bending_level_ + 1);
	vector<double> bending_gains(bending_level_ + 1);
	vector<int> sort_indices(bending->data_.size());
	iota(begin(sort_indices), end(sort_indices), 0);
	std::sort(begin(sort_indices), end(sort_indices),
		[&](size_t v1, size_t v2) {
		return bending->data_[v1] < bending->data_[v2];
	});

	int step = std::floor((double)bending->data_.size() / bending_level_);
	for (int i = 0; i <= bending_level_; ++i)
	{
		bending_level[i] = i * step;
		bending_gains[i] = i * max_bending_gain_ / bending_level_;
	}
	bending_level[bending_level_] = bending->data_.size() - 1;
	for (int i = 0; i <= bending_level_; ++i)
	{
		int pos = sort_indices[bending_level[i]];
		bending_interval[i] = bending->data_[pos];
	}
	std::transform(begin(bending->data_), end(bending->data_),
		begin(bending->data_), [&, this](const float& v) -> float {
		double min_bound, max_bound;
		double min_bend, max_bend;
		int interval_pos = 0;
		for (int i = 0; i <= bending_level_; ++i)
		{
			if (v <= bending_interval[i])
			{
				interval_pos = i - 1;
				break;
			}
		}
		if (interval_pos > bending_level_)
			interval_pos = bending_level_;
		if (interval_pos < 0)
			interval_pos = 0;
		min_bound = bending_interval[interval_pos];
		max_bound = bending_interval[interval_pos + 1];
		min_bend = bending_gains[interval_pos];
		max_bend = bending_gains[interval_pos + 1];

		return (v - min_bound) / (max_bound - min_bound) *
			(max_bend - min_bend) + min_bend;
	});
}


template <typename PointT>
vector<uint8_t> oil::AdaptiveSurfaceFilter<PointT>::getClassification()
{
	//1 unclassified 2 ground 7 low points
	int num_total = input_->size();
	vector<uint8_t> lables(num_total);

	for (int i = 0; i < low_outlier_index_.size(); ++i)
	{
		lables[low_outlier_index_[i]] = 7;
	}
	for (int i = 0; i < ground_index_.size(); ++i)
	{
		lables[ground_index_[i]] = 2;
	}
	for (int i = 0; i < nonground_index_.size();++i)
	{
		lables[nonground_index_[i]] = 1;
	}
	return lables;
}


#define PCL_INSTANTIATE_AdaptiveSurfaceFilter(T) template class PCL_EXPORTS AdaptiveSurfaceFilter<T>;
}